/*
 * AvoidObstacleAction.cpp
 *
 *  Created on: 17/lug/2014
 *      Author: va2013
 */

#include "AvoidObstacleAction.h"

AvoidObstacleAction::AvoidObstacleAction(ArRobot* r, double turningTime, int rotVel,
		bool* mustRealign) :
		ArAction("AvoidObstacleAction") {
	robot = r;
	this->turningTime = turningTime;
	this->mustRealign = mustRealign;
	this->startTime = cv::getTickCount();
	this->rotationVel = rotVel;
	this->achievedCurve = false;
	this->init = true;
	this->actualRotVel = 0;
	direction = 0;


}

AvoidObstacleAction::~AvoidObstacleAction() {
}

ArActionDesired* AvoidObstacleAction::fire(ArActionDesired currentDesired) {


	//myDesired.setRotAccel(25);
	if (!(*mustRealign) && init) {
		this->startTime = cv::getTickCount();
		init = false;
		actualRotVel = rotationVel;
		cout <<"prima fase" << endl;
	} else if ((*mustRealign) && !init) {
		init=true;
		this->startTime = cv::getTickCount();
		direction *= -1;
		actualRotVel = rotationVel;
		turningTime *=2;
		cout <<"mustRealign" << *mustRealign<<endl;
	}

	double elapsed = (cv::getTickCount() - startTime) / cv::getTickFrequency();
	if (elapsed >= turningTime) {
		actualRotVel = 0;
		if(*mustRealign)
			achievedCurve = true;
	}

	myDesired.setRotVel((double) (direction) * actualRotVel, 0.5);
	std::cout <<" rotvel desired: " << (double) (direction) * actualRotVel << std::endl;

	return &myDesired;

}

void AvoidObstacleAction::setDeltaHeading(double turningTime, int direction) {

	this->turningTime = turningTime;
	this->startTime = cv::getTickCount();
	achievedCurve = false;
	this->direction = direction;
	init = true;
	this->actualRotVel = 0;

}
bool AvoidObstacleAction::haveAchievedCurve() {
	return achievedCurve;
}

